# -*- encoding:UTF-8 -*-
from naoqi import ALProxy
import time
import threading
import cmath

IP = "192.168.1.103"
PORT = 9559
global motionProxy
global ttsProxy
global postureProxy


def WALKFREQUENCY():
    maxstepx = 0.08
    maxstepy = 0.1
    maxsteptheta = 0.2
    # maxstepfrequency = 0.6
    maxstepfrequency = 0.4
    stepheight = 0.02
    torsowx = 0.0
    torsowy = 0.0
    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]


def standUp():
    postureProxy.goToPosture("Stand", 0.5)
    pass


def stretchArms():
    # ttsProxy.say("伸展手臂")
    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", 73.4 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 4.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -4.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RHand", 1.0, 0.2)
    motionProxy.setAngles("LHand", 1.0, 0.2)
    time.sleep(1)
    pass


def curlArms():
    # ttsProxy.say("弯曲手臂")
    motionProxy.setAngles("RShoulderPitch", 0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", 0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LShoulderRoll", 70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -2.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # motionProxy.setAngles("RWristYaw", 100 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LWristYaw", -100 * cmath.pi / 180, 0.2)
    # time.sleep(1)

    # motionProxy.setAngles("RElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    # motionProxy.setAngles("LElbowYaw", 0.0 * cmath.pi / 180, 0.2)
    pass


def catchTheBall():
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    # ttsProxy.say("抓取小球")
    motionProxy.setAngles("RElbowRoll", 15.0 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LElbowRoll", -15.0 * cmath.pi / 180, 0.2)
    time.sleep(1)

    motionProxy.setAngles("RShoulderRoll", 15 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LShoulderRoll", -15 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RHand", 0.3, 0.2)
    motionProxy.setAngles("LHand", 0.3, 0.2)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -20 * cmath.pi / 180, 0.2)
    motionProxy.setAngles("LShoulderPitch", -20 * cmath.pi / 180, 0.2)
    time.sleep(5)
    pass


def backCurlArms():
    # 版本1
    # ttsProxy.say("收回手臂")
    # motionProxy.setAngles("RShoulderPitch", -50 * cmath.pi / 180, 0.05)
    # motionProxy.setAngles("LShoulderPitch", -50 * cmath.pi / 180, 0.05)
    # time.sleep(3)
    #
    # RShoudlerRollTheta = 11.2
    # LShoudlerRollTheta = -11.2
    # RElbowRollTheta = 13.7
    # LElbowRollTheta = -13.7
    # for i in range(10):
    #     RShoudlerRollTheta = RShoudlerRollTheta - 0.3
    #     LShoudlerRollTheta = LShoudlerRollTheta + 0.3
    #     motionProxy.setAngles("RShoulderRoll", RShoudlerRollTheta * cmath.pi / 180, 0.05)
    #     motionProxy.setAngles("LShoulderRoll", LShoudlerRollTheta * cmath.pi / 180, 0.05)
    #
    #     RElbowRollTheta = RElbowRollTheta + 3
    #     LElbowRollTheta = LElbowRollTheta - 3
    #     motionProxy.setAngles("RElbowRoll", RElbowRollTheta * cmath.pi / 180, 0.05)
    #     motionProxy.setAngles("LElbowRoll", LElbowRollTheta * cmath.pi / 180, 0.05)
    #
    # motionProxy.setAngles("RShoulderPitch", -18 * cmath.pi / 180, 0.05)
    # motionProxy.setAngles("LShoulderPitch", -18 * cmath.pi / 180, 0.05)

    # 版本2(右手粘胶)

    pass


def move():
    motionProxy.setMoveArmsEnabled(False, False)
    # motionProxy.wakeUp()
    # motionProxy.move(0.1, 0.0, 0.0, moveConfig)
    for i in range(10):
        motionProxy.move(0, 0.1, 0.0, moveConfig)
        time.sleep(1)


class turnAround(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)

    def run(self):
        motionProxy.setMoveArmsEnabled(False, False)
        initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
        while True:
            motionProxy.move(0, 0, 0.1, moveConfig)
            nowtangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
            dis = (nowtangle - initangle) * 180
            print dis
            time.sleep(1)


def back():
    ttsProxy.say("back")
    motionProxy.move(-0.1, 0.0, 0.0, moveConfig)
    time.sleep(7)
    motionProxy.stopMove()
    time.sleep(2)
    pass


def catchTheSmallBall():
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LShoulderPitch", -8.1 * cmath.pi / 180, 0.1)
    time.sleep(1)

    # ttsProxy.say("小小球")
    motionProxy.setAngles("RElbowRoll", 15.0 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LElbowRoll", -15.0 * cmath.pi / 180, 0.1)
    time.sleep(1)

    # 先伸右手
    motionProxy.setAngles("RWristYaw", 60 * cmath.pi / 180, 0.1)
    time.sleep(1)
    motionProxy.setAngles("RShoulderRoll", -12.5 * cmath.pi / 180, 0.05)
    time.sleep(5)
    motionProxy.setAngles("RElbowRoll", 65.5 * cmath.pi / 180, 0.05)
    time.sleep(3)
    motionProxy.setAngles("RHand", 1, 0.1)
    time.sleep(1)

    # 攥紧左手
    motionProxy.setAngles("LHand", 0, 0.1)
    time.sleep(1)
    motionProxy.setAngles("LElbowRoll", -47 * cmath.pi / 180, 0.1)
    time.sleep(1)
    motionProxy.setAngles("LShoulderRoll", -8 * cmath.pi / 180, 0.05)
    time.sleep(5)
    # 右手手指开始抓取
    motionProxy.setAngles("RHand", 0, 0.05)

    # # 以前代码
    # motionProxy.setAngles("LWristYaw", 90 * cmath.pi / 180, 0.1)
    # time.sleep(1)
    #
    # motionProxy.setAngles("LShoulderRoll", -10 * cmath.pi / 180, 0.05)
    # time.sleep(5)
    #
    # # 挤压
    # motionProxy.setAngles("RElbowRoll", 36 * cmath.pi / 180, 0.05)
    # motionProxy.setAngles("LElbowRoll", -36 * cmath.pi / 180, 0.05)
    # time.sleep(3)
    #
    # motionProxy.setAngles("RHand", 0.3, 0.2)
    # # 不设置左手手指
    # # motionProxy.setAngles("LHand", 0.3, 0.2)
    # time.sleep(3)
    #
    # # 放下左臂(移动右臂)
    # motionProxy.setAngles("LElbowRoll", -2 * cmath.pi / 180, 0.05)
    # motionProxy.setAngles("RElbowRoll", 2 * cmath.pi / 180, 0.05)
    # time.sleep(1)
    # motionProxy.setAngles("LShoulderRoll", 75 * cmath.pi / 180, 0.1)
    # motionProxy.setAngles("RShoulderRoll", -75 * cmath.pi / 180, 0.1)
    # time.sleep(3)
    # motionProxy.setAngles("LShoulderPitch", 105 * cmath.pi / 180, 0.1)
    # motionProxy.setAngles("RShoulderPitch", 73 * cmath.pi / 180, 0.1)
    # time.sleep(3)
    # motionProxy.setAngles("LShoulderRoll", 15 * cmath.pi / 180, 0.1)
    # motionProxy.setAngles("RShoulderRoll", -13 * cmath.pi / 180, 0.1)
    # time.sleep(3)

    # 后退或者转向
    pass


def catchTheCylinder():
    # 抓圆柱体(桌子高 37.4  据胸口10cm)
    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RElbowRoll", 2.6 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RElbowYaw", 0 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RHand", 1, 0.1)
    time.sleep(1)
    motionProxy.setAngles("RWristYaw", 82.5 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RShoulderRoll", 0 * cmath.pi / 180, 0.1)
    time.sleep(5)
    motionProxy.setAngles("RElbowRoll", 55 * cmath.pi / 180, 0.1)
    time.sleep(5)
    motionProxy.setAngles("RHand", 0, 0.05)
    time.sleep(5)

    # 放下手臂
    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("LShoulderPitch", 111 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    pass


def putBallDown():
    # 平举手臂
    ttsProxy.say("放下小球")
    motionProxy.setAngles("RShoulderRoll", -75.0 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", -11 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", 18.0 * cmath.pi / 180, 0.1)
    time.sleep(5)
    motionProxy.setAngles("RWristYaw", 0 * cmath.pi / 180, 0.1)
    time.sleep(1)
    motionProxy.setAngles("RHand", 1, 0.2)
    time.sleep(3)
    # 收回手臂
    motionProxy.setAngles("RShoulderRoll", -75.5 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RWristYaw", 73.3 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 73 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -13 * cmath.pi / 180, 0.1)
    time.sleep(3)
    pass


def washFruit():
    # 洗水果
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -10 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 3 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    for i in range(2):
        motionProxy.setAngles("RElbowRoll", 50 * cmath.pi / 180, 0.1)
        time.sleep(2)
        motionProxy.setAngles("RElbowRoll", 2 * cmath.pi / 180, 0.1)
        time.sleep(2)

    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    pass


def putCylinderDown():
    # 放下水果
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -10 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 3 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 50 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RHand", 1, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    pass


if __name__ == '__main__':
    # 双手 后退
    # maxstepx = 0.02
    # maxstepy = 0.03
    # maxsteptheta = 0.2
    # maxstepfrequency = 0.1
    # maxstepheight = 0.02
    # torsowx = 0.0
    # torsowy = 0.0

    # 单手 后退
    maxstepx = 0.02
    maxstepy = 0.03
    maxsteptheta = 0.1
    maxstepfrequency = 0.2
    maxstepheight = 0.05
    torsowx = 0.0
    torsowy = 0.0

    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepheight],
                  ["MaxStepFrequency", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]

    motionProxy = ALProxy("ALMotion", IP, PORT)
    ttsProxy = ALProxy("ALTextToSpeech", IP, PORT)
    postureProxy = ALProxy("ALRobotPosture", IP, PORT)
    memorypxy = ALProxy("ALMemory", IP, PORT)
    motionProxy.wakeUp()
    # motionProxy.moveTo(0.1, 0.0, 0.0)
    # time.sleep(2)

    # 站直
    standUp()

    # 伸展手臂
    # stretchArms()

    # 弯曲手臂
    # curlArms()

    # 抓取小球
    # catchTheBall()
    # catchTheSmallBall()
    catchTheCylinder()

    # 收回手臂
    # backCurlArms()


    # move()





    # 转圈
    # turn = turnAround()
    # turn.start()

    # 移动测试(可用)
    # motionProxy.move(0, 0, 90, moveConfig)
    # time.sleep(7)
    # motionProxy.stopMove()

    # 后退
    # back()

    # 洗水果
    washFruit()

    # 放下小球
    # putBallDown()
    putCylinderDown()
